/**
  *****************************************************************************
  * @Author       : JackyJuu
  * @Site         : https://www.geekros.com
  * @LastEditTime : 2022-03-18 18:02:50
  * @FilePath     : \Geek_Dog\hardware\robot\lite_dog\src\dog_order.c
  * @Description  : 机器狗指令相关代码
  * @Copyright (c) 2022 by GEEKROS, All Rights Reserved. 
  ******************************************************************************
*/
#include "dog_move.h"
#include "dog_order.h"
#include "dog_app.h"
#include "lite_dog.h"
Lite_Dog_Struct* Dog_Data;

Dog_Order_Mode_Struct Dog_Order_Mode;

/*******************************************************************************
  * @funtion      : Dog_Order_Mode_Init  
  * @LastEditors  : JackyJuu
  * @description  : 指令相关控制数据初始化
  * @param         {*}
  * @return        {*}
  *******************************************************************************/
void Dog_Order_Mode_Init(void)
{
    Dog_Data = Get_Lite_Dog_Data_Address();
    Dog_Order_Mode.Dog_Order_Sport_Mode =  Dog_Order_Init_Sport_Mode;
    Dog_Order_Mode.Dog_Order_Set_Mode = Dog_Order_Init_Set_Mode;
    Dog_Order_Mode.Dog_Order_Move_Mode = Dog_Order_Init_Move_Mode;
    Dog_Order_Mode.Order_Mode_Move_Speed_Set = Max_Time_Set/Dog_Order_Init_Move_Gait;
    Dog_Order_Mode.Order_Mode_Move_Height_Set = Dog_Order_Init_Move_Height;
    Dog_Order_Mode.Order_Mode_Move_Lenth_Set = Dog_Order_Init_Move_Lenth;
    Dog_Order_Mode.Order_Mode_XYZ_Set[0] = Dog_Init_Gravity;
    Dog_Order_Mode.Order_Mode_XYZ_Set[1] = Dog_Init_Gravity;
    Dog_Order_Mode.Order_Mode_XYZ_Set[2] = Dog_Init_Height;
    Dog_Order_Mode.Order_Mode_Roll_Set = Dog_Order_Init_Roll;
    Dog_Order_Mode.Order_Mode_Pitch_Set = Dog_Order_Init_Pitch;
    Dog_Order_Mode.Order_Mode_Yaw_Set = Dog_Order_Init_Yaw;
}


/*******************************************************************************
  * @funtion      : Dog_Mode_Serial_Callback  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗模式修改相关回调函数
  * @param         {char} *mode 设置模式
  * @return        {*}
  *******************************************************************************/
void Dog_Mode_Serial_Callback(char *mode)
{
    if(memcmp(mode, "PowerLess", 2) == 0)
    {
        Lite_Dog_Change_Mode(Dog_PowerLess_Mode);
	}
    else if(memcmp(mode, "Remote_Mode", 2) == 0)
    {
        Lite_Dog_Change_Mode(Dog_Remote_Control_Mode);
	}
    else if(memcmp(mode, "Order_Mode", 2) == 0)
    {
        Lite_Dog_Change_Mode(Dog_Order_Control_Mode);
	}
}


/*******************************************************************************
  * @funtion      : Dog_Celebration_Serial_Callback 
  * @LastEditors  : JackyJuu
  * @description  : 机器狗腿部校准回调函数
  * @param         {char} *leg 校准的腿
  * @param         {char} *State 校准状态设置
  * @return        {*}
  *******************************************************************************/
void Dog_Celebration_Serial_Callback(char *leg, char *State)
{
    int Set_Flag;
    Leg_Loc_Enum Leg_Loc_Set;

    if(memcmp(leg, "LF", 2) == 0)
    {
        Set_Flag = 1;
		Leg_Loc_Set = Dog_LF_Leg;
	}
    else if(memcmp(leg, "RF", 2) == 0)
    {
        Set_Flag = 1;
		Leg_Loc_Set = Dog_RF_Leg;
	}
    else if(memcmp(leg, "LB", 2) == 0)
    {
        Set_Flag = 1;
		Leg_Loc_Set = Dog_LB_Leg;
	}
    else if(memcmp(leg, "RB", 2) == 0)
    {
        Set_Flag = 1;
		Leg_Loc_Set = Dog_RB_Leg;
	}
    else
    {
        Set_Flag = 0;
    }

    if(Set_Flag == 1)
    {
        if(memcmp(State, "ready", 5) == 0)
        {
            Lite_Dog_Celebrate_Data_Set(Leg_Loc_Set,Leg_Ready);
        }
        else if(memcmp(State, "butt_done", 4) == 0)
        {
            Lite_Dog_Celebrate_Data_Set(Leg_Loc_Set,Leg_Thigh_Celebrate);
        }
        else if(memcmp(State, "thigh_done", 4) == 0)
        {
            Lite_Dog_Celebrate_Data_Set(Leg_Loc_Set,Leg_Calf_Celebrate);
        }
        else if(memcmp(State, "calf_done", 4) == 0)
        {
            Lite_Dog_Celebrate_Data_Set(Leg_Loc_Set,Leg_Celebrate_Done);
        }
        else if(memcmp(State, "leg_ok", 4) == 0)
        {
            Lite_Dog_Celebrate_Data_Set(Leg_Loc_Set,Leg_OK);
        }
        else if(memcmp(State, "leg_stop", 4) == 0)
        {
            Lite_Dog_Celebrate_Data_Set(Leg_Loc_Set,Leg_Stop);
        }
        
    }
    else
    {
        Usb_Write_Data("Something is Wrong,Please Check your Message!\n");
    } 
}

//姿态控制回调函数
/*******************************************************************************
  * @funtion      : Dog_Attitude_Serial_Callback  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗姿态控制回调函数
  * @param         {float*} XYZ  设置XYZ三方向位置数据
  * @param         {float} roll  设置roll轴角度数据
  * @param         {float} pitch 设置pitch轴角度数据
  * @param         {float} yaw   设置yaw轴角度数据
  * @return        {*}
  *******************************************************************************/
void Dog_Attitude_Serial_Callback(float* XYZ,float roll,float pitch,float yaw)
{
    Lite_Dog_Write_XYZ(XYZ);
    Lite_Dog_Write_Roll(roll);
    Lite_Dog_Write_Pitch(pitch);
    Lite_Dog_Write_Yaw(yaw);
}

//XY控制回调函数
/*******************************************************************************
  * @funtion      : Dog_XYZ_Set_Serial_Callback  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗足端坐标修改控制回调函数
  * @param         {char} *leg  腿部设置
  * @param         {int} x      x轴坐标
  * @param         {int} y      y轴坐标
  * @param         {int} z      z轴坐标
  * @param         {int} speed  速度设置
  * @param         {int} time   时间设置
  * @return        {*}
  *******************************************************************************/
void Dog_XYZ_Set_Serial_Callback(char *leg,int x,int y,int z,int speed,int time)
{
    if(memcmp(leg, "LF", 2) == 0)
    {
        Single_Leg_Coordinate_To_Angle(&Dog_Data->Dog_LF_Leg_Data,x,y,z);
	    Single_Leg_Data_To_Servo(&Dog_Data->Dog_LF_Leg_Data,time,speed);
	}
    else if(memcmp(leg, "RF", 2) == 0)
    {
        Single_Leg_Coordinate_To_Angle(&Dog_Data->Dog_RF_Leg_Data,x,y,z);
	    Single_Leg_Data_To_Servo(&Dog_Data->Dog_RF_Leg_Data,time,speed);
	}
    else if(memcmp(leg, "LB", 2) == 0)
    {
        Single_Leg_Coordinate_To_Angle(&Dog_Data->Dog_LB_Leg_Data,x,y,z);
	    Single_Leg_Data_To_Servo(&Dog_Data->Dog_LB_Leg_Data,time,speed);
	}
    else if(memcmp(leg, "RB", 2) == 0)
    {
        Single_Leg_Coordinate_To_Angle(&Dog_Data->Dog_RB_Leg_Data,x,y,z);
	    Single_Leg_Data_To_Servo(&Dog_Data->Dog_RB_Leg_Data,time,speed);
	}
}


/*******************************************************************************
  * @funtion      : Dog_Control_Serial_Callback  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗运动控制回调函数
  * @param         {int} Cycle  运动周期时长，一个周期运动时间
  * @param         {char} *mode 运动状态设置
  * @param         {char} *control  运动模式设置
  * @return        {*}
  *******************************************************************************/
void Dog_Control_Serial_Callback(int Cycle, char *mode, char *control)
{
    Lite_Dog_Auto_Control_Set(Cycle);

	if(memcmp(mode, "trot", 4) == 0)
    {
		Lite_Dog_Motion_Mode_Set(Trot_M);
	}
    else if(memcmp(mode, "walk", 4) == 0)
    {
		Lite_Dog_Motion_Mode_Set(Walk_M);
	}
    else if(memcmp(mode, "pronk", 5) == 0)
    {
		Lite_Dog_Motion_Mode_Set(Pronk_M);
	}
    else if(memcmp(mode, "stable", 6) == 0)
    {
		Lite_Dog_Motion_Mode_Set(Stable_M);
	}
     else if(memcmp(mode, "init", 4) == 0)
    {
		Lite_Dog_Set_Init();
        Lite_Dog_Motion_Mode_Set(Trot_M);
        Lite_Dog_Motion_Control_Set(Stop_M);
	}

    if(memcmp(control, "forward", 7) == 0)
    {
		Lite_Dog_Motion_Control_Set(Front_M);
	}
    else if(memcmp(control, "back", 4) == 0)
    {
		Lite_Dog_Motion_Control_Set(Back_M);
	}
    else if(memcmp(control, "left", 4) == 0)
    {
		Lite_Dog_Motion_Control_Set(Left_M);
	}
    else if(memcmp(control, "right", 5) == 0)
    {
		Lite_Dog_Motion_Control_Set(Right_M);
	}
    else if(memcmp(control, "stop", 4) == 0)
    {
		Lite_Dog_Motion_Control_Set(Stop_M);
	}
    else if(memcmp(control, "step", 4) == 0)
    {
		Lite_Dog_Motion_Control_Set(Step_M);
	}
	
}

/*******************************************************************************
  * @funtion      : Dog_Auto_Mode_Serial_Callback
  * @LastEditors  : JackyJuu
  * @description  : 机器狗自动模式切换回调函数
  * @param         {char} *Auto_Mode 自动模式设置
  * @return        {*}
  *******************************************************************************/
void Dog_Auto_Mode_Serial_Callback(char *Auto_Mode)
{
    if(memcmp(Auto_Mode, "PushUp", 6) == 0)
    {
		Lite_Dog_Auto_Control_Mode_Set(PushUp_Auto);
	}
    else if(memcmp(Auto_Mode, "SayHello", 8) == 0)
    {
		Lite_Dog_Auto_Control_Mode_Set(SayHello_Auto);
	}
    else if(memcmp(Auto_Mode, "Dance", 5) == 0)
    {
		Lite_Dog_Auto_Control_Mode_Set(Dance_Auto);
	}
    else if(memcmp(Auto_Mode, "Pee", 3) == 0)
    {
		Lite_Dog_Auto_Control_Mode_Set(Pee_Auto);
	}
}


/*******************************************************************************
  * @funtion      : Lite_Dog_Auto_Control_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗设置指令模式步态周期
  * @param         {float} Set_Gait 设置周期时长，ms位单位
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Auto_Control_Set(float Set_Gait)
{
    Dog_Order_Mode.Order_Mode_Move_Speed_Set = Max_Time_Set/Set_Gait;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Motion_Mode_Set  
  * @LastEditors  : JackyJuu
  * @description  : 设置机器狗步态模式
  * @param         {Dog_Sport_Mode_Enum} Dog_Sport_Mode_Set 步态模式设置
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Motion_Mode_Set(Dog_Sport_Mode_Enum Dog_Sport_Mode_Set)
{
    switch(Dog_Sport_Mode_Set)
    {
        case Cailbration_M:
            Dog_Order_Mode.Dog_Order_Set_Mode = Dog_Order_Celebrate_Mode;
            Dog_Order_Mode.Dog_Order_Sport_Mode = Dog_Sport_Mode_Set;
            Dog_Order_Mode.Leg_Celebrate_State = Dog_OK;
        break;
        case Stable_M:
            Dog_Order_Mode.Dog_Order_Set_Mode = Dog_Order_Stable_Mode;
            Dog_Order_Mode.Dog_Order_Sport_Mode = Dog_Sport_Mode_Set;
        break;
        case Trot_M:
        case Walk_M:
        case Pronk_M:
            Dog_Order_Mode.Dog_Order_Set_Mode = Dog_Order_Move_Mode;
            Dog_Order_Mode.Dog_Order_Sport_Mode = Dog_Sport_Mode_Set;
        break;
    }
}


/*******************************************************************************
  * @funtion      : Lite_Dog_Motion_Control_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗运动模式修改
  * @param         {Dog_Move_Mode_Enum} Dog_Move_Mode_Set 设置机器狗运动模式
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Motion_Control_Set(Dog_Move_Mode_Enum Dog_Move_Mode_Set)
{
    Dog_Order_Mode.Dog_Order_Move_Mode = Dog_Move_Mode_Set;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Auto_Control_Mode_Set  
  * @LastEditors  : JackyJuu
  * @description  : 设置机器狗自动运动模式
  * @param         {Dog_Order_Auto_Mode_Struct} Dog_Move_Auto_Mode_Set 机器狗自动运动模式修改
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Auto_Control_Mode_Set(Dog_Order_Auto_Mode_Struct Dog_Move_Auto_Mode_Set)
{
    Dog_Order_Mode.Dog_Order_Set_Mode =  Dog_Order_Auto_Mode;
    Dog_Order_Mode.Dog_Order_Auto = Dog_Move_Auto_Mode_Set;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Write_Roll  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗指令模式设置Roll轴滚转角度
  * @param         {float} Dog_Roll_Set 机器狗指令模式Roll角度设置
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Write_Roll(float Dog_Roll_Set)
{
    Dog_Order_Mode.Order_Mode_Roll_Set = Dog_Roll_Set;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Write_Pitch  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗设置指令模式Pitch轴俯仰角度
  * @param         {float} Dog_Pitch_Set 机器狗指令模式Pitch角度设置
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Write_Pitch(float Dog_Pitch_Set)
{
    Dog_Order_Mode.Order_Mode_Pitch_Set = Dog_Pitch_Set;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Write_Yaw  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗设置指令模式Yaw轴俯仰角度
  * @param         {float} Dog_Yaw_Set 机器狗指令模式Yaw角度设置
  * @return        {*} 
  *******************************************************************************/
void Lite_Dog_Write_Yaw(float Dog_Yaw_Set)
{
    Dog_Order_Mode.Order_Mode_Yaw_Set = Dog_Yaw_Set;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Write_XYZ  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗设置指令模式XYZ方向位置信息
  * @param         {float*} XYZ_Data 机器狗指令模式XYZ方向位置设置
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Write_XYZ(float* XYZ_Data)
{
    Dog_Order_Mode.Order_Mode_XYZ_Set[0] = XYZ_Data[0];
    Dog_Order_Mode.Order_Mode_XYZ_Set[1] = XYZ_Data[1];
    Dog_Order_Mode.Order_Mode_XYZ_Set[2] = XYZ_Data[2];
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Celebrate_Data_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗指令模式校准相关设置函数
  * @param         {Leg_Loc_Enum} Set_Leg
  * @param         {Leg_Celebrate_State_Enum} Set_State
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Celebrate_Data_Set(Leg_Loc_Enum Set_Leg,Leg_Celebrate_State_Enum Set_State)
{
    Dog_Order_Mode.Dog_Order_Set_Mode = Dog_Order_Celebrate_Mode;
    Dog_Order_Mode.Dog_Order_Sport_Mode = Cailbration_M;
    Dog_Order_Mode.Leg_Celebrate_Loc_Set = Set_Leg;
    Dog_Order_Mode.Leg_Celebrate_State = Set_State;
}

/*

int Auto_Move_Flag = 0;
int Auto_Move_Time = 0;
float Get_Angle[2];
float Angle_Set_A[2];
float Angle_Set_B[2];
#define PushUp_Time_Set 400
#define PushUp_Time (PushUp_Time_Set/5)
//俯卧撑
void Lite_Dog_Action_PushUp(int Move_Time,Lite_Dog_Struct* Dog_PushUp)
{

    Set_All_Same_XY(0,50,Dog_PushUp);
    Dog_All_Legs_Control_Data_Send(Dog_PushUp,0,50); 
    vTaskDelay(1000);

	Lite_Dog_Servo_To_Data(Dog_PushUp);
    Get_Angle[0] = Dog_PushUp->Dog_LF_Leg_Data.Leg_Angle_Now[2];
    Get_Angle[1] = Dog_PushUp->Dog_RF_Leg_Data.Leg_Angle_Now[2];

    for(int i = 0;i < Move_Time;i ++)
    {
        for(int k = 1;k <= PushUp_Time;k++)
        {	
            Angle_Set_A[0] = 40 + (float)k*60/PushUp_Time;
            Single_Leg_Angle_To_Cood(0,Angle_Set_A[0],0,&Dog_PushUp->Dog_LF_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_LF_Leg_Data,0,0);

            Angle_Set_A[1] = 40 + (float)k*60/PushUp_Time;
            Single_Leg_Angle_To_Cood(0,Angle_Set_A[1],0,&Dog_PushUp->Dog_RF_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_RF_Leg_Data,0,0);

            Angle_Set_B[0] = 150 + (float)k*30/PushUp_Time;
            Set_Leg_A_Angle(Angle_Set_B[0],&Dog_PushUp->Dog_LB_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_LB_Leg_Data,0,0);

            Angle_Set_B[1] = 150 + (float)k*30/PushUp_Time;
            Set_Leg_A_Angle(Angle_Set_B[1],&Dog_PushUp->Dog_RB_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_RB_Leg_Data,0,0);

            vTaskDelay(5);
        }
		vTaskDelay(100);
        for(int k = PushUp_Time;k >= 1;k--)
        {
            Angle_Set_A[0] = 40 + (float)k*60/PushUp_Time;
            Single_Leg_Angle_To_Cood(0,Angle_Set_A[0],0,&Dog_PushUp->Dog_LF_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_LF_Leg_Data,0,0);

            Angle_Set_A[1] = 40 + (float)k*60/PushUp_Time;
            Single_Leg_Angle_To_Cood(0,Angle_Set_A[1],0,&Dog_PushUp->Dog_RF_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_RF_Leg_Data,0,0);

            Angle_Set_B[0] = 150 + (float)k*30/PushUp_Time;
            Single_Leg_Angle_To_Cood(0,0,Angle_Set_B[0],&Dog_PushUp->Dog_LB_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_LB_Leg_Data,0,0);

            Angle_Set_B[1] = 150 + (float)k*30/PushUp_Time;
            Single_Leg_Angle_To_Cood(0,0,Angle_Set_B[1],&Dog_PushUp->Dog_RB_Leg_Data);
            Single_Leg_Data_To_Servo(&Dog_PushUp->Dog_RB_Leg_Data,0,0);
            vTaskDelay(5);
        }
		vTaskDelay(100);
    }

    Set_All_Same_XY(Dog_Init_Gravity,Dog_Init_Height,Dog_PushUp);
    Dog_All_Legs_Control_Data_Send(Dog_PushUp,0,100); 
    vTaskDelay(1000);
}

float Say_Hello_Angle_Set;
#define SayHello_Start_Angle 30
#define SayHello_Angle_Set 20
#define SayHello_Time_Set 100
#define SayHello_Time   (SayHello_Time_Set/5)

//握个手，打招呼
void Lite_Dog_Action_SayHello(int Move_Time,int Hand_Set,Lite_Dog_Struct* Dog_SayHello)
{

    Set_Leg_AB_Angle(100,110,&Dog_SayHello->Dog_LF_Leg_Data);
    Set_Leg_AB_Angle(100,110,&Dog_SayHello->Dog_RF_Leg_Data);
    Set_Leg_AB_Angle(150,30,&Dog_SayHello->Dog_LB_Leg_Data);
    Set_Leg_AB_Angle(150,30,&Dog_SayHello->Dog_RB_Leg_Data);
    Set_All_Motor_XY_Angle(Dog_SayHello);
    Send_All_Legs_Motor_Data(Dog_SayHello,0,100);
    vTaskDelay(1000);


    if(Hand_Set == 1)
    {
        Set_Leg_AB_Angle(30,SayHello_Start_Angle,&Dog_SayHello->Dog_LF_Leg_Data);
        Set_Motor_XY_Angle(&Dog_SayHello->Dog_LF_Leg_Data);
        Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_LF_Leg_Data,0,400);
        vTaskDelay(500);

        Get_Legs_Motor_Message(&Dog_SayHello->Dog_LF_Leg_Data);
        Get_Angle[0] = Dog_SayHello->Dog_LF_Leg_Data.Leg_Angle_Now[2];
        for(int i = 0;i < Move_Time;i ++)
        {
            for(int k = 1;k <= SayHello_Time;k++)
            {    
                Say_Hello_Angle_Set = SayHello_Start_Angle + (float)k*SayHello_Angle_Set/SayHello_Time;
                Set_Leg_B_Angle(Say_Hello_Angle_Set,&Dog_SayHello->Dog_LF_Leg_Data);
                Set_Motor_XY_Angle(&Dog_SayHello->Dog_LF_Leg_Data);
                Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_LF_Leg_Data,0,0);
                vTaskDelay(5);
            }

            for(int k = SayHello_Time;k > 0;k--)
            {    
                Say_Hello_Angle_Set = SayHello_Start_Angle + (float)k*SayHello_Angle_Set/SayHello_Time;
                Set_Leg_B_Angle(Say_Hello_Angle_Set,&Dog_SayHello->Dog_LF_Leg_Data);
                Set_Motor_XY_Angle(&Dog_SayHello->Dog_LF_Leg_Data);
                Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_LF_Leg_Data,0,0);
                vTaskDelay(5);
            }   
        }
        Set_Leg_AB_Angle(100,110,&Dog_SayHello->Dog_LF_Leg_Data);
        Set_Motor_XY_Angle(&Dog_SayHello->Dog_LF_Leg_Data);
        Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_LF_Leg_Data,0,100);
        vTaskDelay(1000);

    }
    else if(Hand_Set == 2)
    {
        Set_Leg_AB_Angle(30,SayHello_Start_Angle,&Dog_SayHello->Dog_RF_Leg_Data);
        Set_Motor_XY_Angle(&Dog_SayHello->Dog_RF_Leg_Data);
        Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_RF_Leg_Data,0,400);
        vTaskDelay(500);

        Get_Legs_Motor_Message(&Dog_SayHello->Dog_RF_Leg_Data);
        Get_Angle[0] = Dog_SayHello->Dog_RF_Leg_Data.Leg_Angle_Now[2];
        for(int i = 0;i < Move_Time;i ++)
        {
            for(int k = 1;k <= SayHello_Time;k++)
            {    
                Say_Hello_Angle_Set = SayHello_Start_Angle + (float)k*SayHello_Angle_Set/SayHello_Time;
                Set_Leg_B_Angle(Say_Hello_Angle_Set,&Dog_SayHello->Dog_RF_Leg_Data);
                Set_Motor_XY_Angle(&Dog_SayHello->Dog_RF_Leg_Data);
                Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_RF_Leg_Data,0,0);
                vTaskDelay(5);
            }
            for(int k = SayHello_Time;k > 0;k--)
            {    
                Say_Hello_Angle_Set = SayHello_Start_Angle + (float)k*SayHello_Angle_Set/SayHello_Time;
                Set_Leg_B_Angle(Say_Hello_Angle_Set,&Dog_SayHello->Dog_RF_Leg_Data);
                Set_Motor_XY_Angle(&Dog_SayHello->Dog_RF_Leg_Data);
                Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_RF_Leg_Data,0,0);
                vTaskDelay(5);
            }   
        }
        Set_Leg_AB_Angle(100,110,&Dog_SayHello->Dog_LF_Leg_Data);
        Set_Motor_XY_Angle(&Dog_SayHello->Dog_LF_Leg_Data);
        Single_Leg_Data_To_Servo(&Dog_SayHello->Dog_LF_Leg_Data,0,100);
        vTaskDelay(1000);

    }
    
    Set_All_Same_XY(Dog_Init_Gravity,Dog_Init_Height,Dog_SayHello);
    Dog_All_Legs_Control_Data_Send(Dog_SayHello,0,100); 
    vTaskDelay(1000);
}

float Dance_Angle_Set;
#define Dance_Height_Set 110.00f
#define Dance_Height_Add_Set 30.00f
#define Dance_Time_Set 300
#define Dance_Time (Dance_Time_Set/5)
float Dance_Pitch_Set,Dance_Roll_Set,Set_LR;
float X_Set[4] = {0,0,0,0};
float Y_Set[4] = {Dance_Height_Set,Dance_Height_Set,Dance_Height_Set,Dance_Height_Set};

//扭一扭，舞蹈
void Lite_Dog_Action_Dance(int Move_Time,int Turn_Set,Lite_Dog_Struct* Dog_Dance)
{
    Set_All_Legs_XY_Cood(X_Set,Y_Set,Dog_Dance);
    Set_All_Motor_XY_Angle(Dog_Dance);
    Send_All_Legs_Motor_Data(Dog_Dance,250,0);
    vTaskDelay(1000);
    
    if(Turn_Set == 1)
    {
        Set_LR = 1;
    }
    else if(Turn_Set == 2)
    {
        Set_LR = -1;
    }

    for(int k = 0;k <= Dance_Time;k++)
    {  
        Dance_Pitch_Set = -(float)k * Dance_Height_Add_Set/Dance_Time; 
        Y_Set[0] = Dance_Height_Set + Dance_Pitch_Set;
        Y_Set[1] = Dance_Height_Set + Dance_Pitch_Set;
        Y_Set[2] = Dance_Height_Set - Dance_Pitch_Set;
        Y_Set[3] = Dance_Height_Set - Dance_Pitch_Set;
        Set_All_Legs_XY_Cood(X_Set,Y_Set,Dog_Dance);
        Set_All_Motor_XY_Angle(Dog_Dance);
        Send_All_Legs_Motor_Data(Dog_Dance,0,0);
        vTaskDelay(5);    
    }
    for(int i = 0;i < Move_Time;i ++)
    {
        for(int k = Dance_Time;k >= 0;k--)
        {  
            Dance_Pitch_Set = -(float)k * Dance_Height_Add_Set/Dance_Time;
            Dance_Roll_Set = Set_LR * (float)(Dance_Time - (float)k) * Dance_Height_Add_Set/Dance_Time;
            Y_Set[0] = Dance_Height_Set + Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[1] = Dance_Height_Set + Dance_Pitch_Set + Dance_Roll_Set;
            Y_Set[2] = Dance_Height_Set - Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[3] = Dance_Height_Set - Dance_Pitch_Set + Dance_Roll_Set;
            Set_All_Legs_XY_Cood(X_Set,Y_Set,Dog_Dance);
            Set_All_Motor_XY_Angle(Dog_Dance);
            Send_All_Legs_Motor_Data(Dog_Dance,0,0);
            vTaskDelay(5);  
        }

        for(int k = 0;k <= Dance_Time;k++)
        {  
            Dance_Pitch_Set = (float)k * Dance_Height_Add_Set/Dance_Time;
            Dance_Roll_Set = Set_LR * (float)(Dance_Time - (float)k) * Dance_Height_Add_Set/Dance_Time;
            Y_Set[0] = Dance_Height_Set + Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[1] = Dance_Height_Set + Dance_Pitch_Set + Dance_Roll_Set;
            Y_Set[2] = Dance_Height_Set - Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[3] = Dance_Height_Set - Dance_Pitch_Set + Dance_Roll_Set;
            Set_All_Legs_XY_Cood(X_Set,Y_Set,Dog_Dance);
            Set_All_Motor_XY_Angle(Dog_Dance);
            Send_All_Legs_Motor_Data(Dog_Dance,0,0);
            vTaskDelay(5);  
            
        }

        for(int k = Dance_Time;k >= 0;k--)
        {  
            Dance_Pitch_Set = (float)k * Dance_Height_Add_Set/Dance_Time;
            Dance_Roll_Set = -Set_LR * (Dance_Time - (float)k) * Dance_Height_Add_Set/Dance_Time;
            Y_Set[0] = Dance_Height_Set + Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[1] = Dance_Height_Set + Dance_Pitch_Set + Dance_Roll_Set;
            Y_Set[2] = Dance_Height_Set - Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[3] = Dance_Height_Set - Dance_Pitch_Set + Dance_Roll_Set;
            Set_All_Legs_XY_Cood(X_Set,Y_Set,Dog_Dance);
            Set_All_Motor_XY_Angle(Dog_Dance);
            Send_All_Legs_Motor_Data(Dog_Dance,0,0);
            vTaskDelay(5);
            
        }

        for(int k = 0;k <= Dance_Time;k++)
        {  
            Dance_Pitch_Set = -(float)k * Dance_Height_Add_Set/Dance_Time;
            Dance_Roll_Set = -Set_LR * (float)(Dance_Time - k) * Dance_Height_Add_Set/Dance_Time;
            Y_Set[0] = Dance_Height_Set + Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[1] = Dance_Height_Set + Dance_Pitch_Set + Dance_Roll_Set;
            Y_Set[2] = Dance_Height_Set - Dance_Pitch_Set - Dance_Roll_Set;
            Y_Set[3] = Dance_Height_Set - Dance_Pitch_Set + Dance_Roll_Set;
            Set_All_Legs_XY_Cood(X_Set,Y_Set,Dog_Dance);
            Set_All_Motor_XY_Angle(Dog_Dance);
            Send_All_Legs_Motor_Data(Dog_Dance,0,0);
            vTaskDelay(5);  
            
        } 
    }    
    for(int k = Dance_Time;k >= 0;k--)
    {  
        Dance_Pitch_Set = -(float)k * Dance_Height_Add_Set/Dance_Time; 
        Y_Set[0] = Dance_Height_Set + Dance_Pitch_Set;
        Y_Set[1] = Dance_Height_Set + Dance_Pitch_Set;
        Y_Set[2] = Dance_Height_Set - Dance_Pitch_Set;
        Y_Set[3] = Dance_Height_Set - Dance_Pitch_Set;
        Set_All_Legs_XY_Cood(X_Set,Y_Set,Dog_Dance);
        Set_All_Motor_XY_Angle(Dog_Dance);
        Send_All_Legs_Motor_Data(Dog_Dance,0,0);
        vTaskDelay(5);    
    }

    Set_All_Same_XY(Dog_Init_Gravity,Dog_Init_Height,Dog_Dance);
    Dog_All_Legs_Control_Data_Send(Dog_Dance,0,100); 
    vTaskDelay(1000);
}
*/

/*******************************************************************************
  * @funtion      : Get_Order_Mode_Address  
  * @LastEditors  : JackyJuu
  * @description  : 获取指令控制相关数据结构体地址
  * @param         {*}
  * @return        {*}
  *******************************************************************************/
Dog_Order_Mode_Struct* Get_Order_Mode_Address(void)
{
    return &Dog_Order_Mode;
}

/*******************************************************************************
  * @funtion      : Dog_Order_Motion_Data_Get  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗指令模式设置运动相关数值
  * @param         {Dog_Order_Mode_Struct*} Order_Data_Get 指令模式数据相关结构体地址
  * @param         {Dog_Sport_Mode_Enum*} Sport_Mode_Set   设置指令模式运动状态
  * @param         {Dog_Move_Mode_Enum*} Move_Mode_Set     设置指令模式运动模式
  * @param         {float*} Move_Height_Get 指令模式步态高度设置
  * @param         {float*} Move_Lenth_Get  指令模式步态长度设置
  * @param         {float*} Move_Speed_Get  指令模式步态速度设置
  * @return        {*}
  *******************************************************************************/
void Dog_Order_Motion_Data_Get(Dog_Order_Mode_Struct* Order_Data_Get,Dog_Sport_Mode_Enum* Sport_Mode_Set,Dog_Move_Mode_Enum* Move_Mode_Set,float* Move_Height_Get,float* Move_Lenth_Get,float* Move_Speed_Get)
{
	if(Order_Data_Get->Dog_Order_Move_Mode != Auto_M)
	{
		*Sport_Mode_Set = Order_Data_Get->Dog_Order_Sport_Mode;
		*Move_Mode_Set = Order_Data_Get->Dog_Order_Move_Mode;
		*Move_Speed_Get = Order_Data_Get->Order_Mode_Move_Speed_Set;
		*Move_Lenth_Get = Order_Data_Get->Order_Mode_Move_Lenth_Set;
		*Move_Height_Get = Order_Data_Get->Order_Mode_Move_Height_Set;
	}
}

/*******************************************************************************
  * @funtion      : Dog_Order_Motion_Move_Data_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗指令模式运动相关数据设置
  * @param         {Lite_Dog_Struct*} Dog_Order_MMS 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Dog_Order_Motion_Move_Data_Set(Lite_Dog_Struct* Dog_Order_MMS)
{
	static Dog_Move_Mode_Enum Dog_Move_Mode_Last;
	static Dog_Sport_Mode_Enum Dog_Sport_Mode_Last;	
	Dog_Move_Mode_Enum Dog_Move_Mode_Set;
	Dog_Sport_Mode_Enum Dog_Sport_Mode_Set;
	float Dog_Motion_Set_Speed,Dog_Motion_Set_Lenth,Dog_Motion_Set_Height;
	
	//获取控制值
	Dog_Order_Motion_Data_Get(Dog_Order_MMS->Dog_Control_Data.Dog_Order_Mode_Data,&Dog_Sport_Mode_Set,&Dog_Move_Mode_Set,&Dog_Motion_Set_Height,&Dog_Motion_Set_Lenth,&Dog_Motion_Set_Speed);
	//发送控制值
	Dog_Motion_Data_Set(&Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data,Dog_Sport_Mode_Set,Dog_Move_Mode_Set,Dog_Motion_Set_Height,Dog_Motion_Set_Lenth,Dog_Motion_Set_Speed);
	
	if(Dog_Sport_Mode_Last != Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Sport_Mode)
	{
			Lite_Dog_Set_Coordinate(Dog_Order_MMS,Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2]);
			Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Motion_Count_Time = 0;
			Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Move_Flag = 1;
			Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Move_Start_Flag = 1;
			Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Motion_Count_Time = 0;		
	}
	if(Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode == Stop_M)
	{
		if(Dog_Move_Mode_Last != Stop_M)
		{
			Lite_Dog_Set_Coordinate(Dog_Order_MMS,Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2]);
		}
		if((Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Sport_Mode != Stable_M) || (Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Sport_Mode != Cailbration_M))
		{
            Lite_Dog_Set_Attitude_XYZ(Dog_Order_MMS,Dog_Order_MMS->Dog_Control_Data.Dog_Order_Mode_Data->Order_Mode_XYZ_Set[0],Dog_Order_MMS->Dog_Control_Data.Dog_Order_Mode_Data->Order_Mode_XYZ_Set[1],Dog_Order_MMS->Dog_Control_Data.Dog_Order_Mode_Data->Order_Mode_XYZ_Set[2]);
			Lite_Dog_Attitude_To_Coordinate(Dog_Order_MMS,Dog_Order_MMS->Dog_Set_XY_Coordinate[0],Dog_Order_MMS->Dog_Set_XY_Coordinate[1],Dog_Order_MMS->Dog_Set_XY_Coordinate[2]);
            Lite_Dog_Get_Coordinate(Dog_Order_MMS,Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2]);
        }
	}
	else
	{
		if(Dog_Move_Mode_Last == Stop_M)
		{
			Lite_Dog_Get_Coordinate(Dog_Order_MMS,Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2]);
		}
		if(Dog_Move_Mode_Last != Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode)
		{
			Lite_Dog_Set_Coordinate(Dog_Order_MMS,Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[0],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[1],Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Leg_Stop_XY_Cood[2]);
			Dog_Move_Mode_Enum Dog_Move_Mode_Step = Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode;
			Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode = Stop_M;
			Dog_Motion_XY_Set(&Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data,Dog_Order_MMS->Dog_Set_XY_Coordinate[1],Dog_Order_MMS->Dog_Set_XY_Coordinate[2]);
			Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode = Dog_Move_Mode_Step;
		}
	}

	Dog_Motion_XY_Set(&Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data,Dog_Order_MMS->Dog_Set_XY_Coordinate[1],Dog_Order_MMS->Dog_Set_XY_Coordinate[2]);
	
	Dog_Move_Mode_Last = Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode;
	Dog_Sport_Mode_Last = Dog_Order_MMS->Dog_Control_Data.Dog_Set_Move_Data.Dog_Sport_Mode;
}

/*******************************************************************************
  * @funtion      : Dog_Order_Adaptive_Angle_Mode_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗指令模式自稳相关设置函数
  * @param         {Lite_Dog_Struct*} Dog_RM_AAM 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Dog_Order_Adaptive_Angle_Mode_Set(Lite_Dog_Struct* Dog_RM_AAM)
{
	float Dog_Y_Add_Data_Get[4];
	
    Lite_Dog_Set_Attitude_Angle(Dog_RM_AAM,Dog_RM_AAM->Dog_Control_Data.Dog_Order_Mode_Data->Order_Mode_Pitch_Set,Dog_RM_AAM->Dog_Control_Data.Dog_Order_Mode_Data->Order_Mode_Roll_Set,0);

    Lite_Dog_Attitude_To_Coordinate(Dog_RM_AAM,Dog_RM_AAM->Dog_Set_XY_Coordinate[0],Dog_RM_AAM->Dog_Set_XY_Coordinate[1],Dog_RM_AAM->Dog_Set_XY_Coordinate[2]);

}

/*******************************************************************************
  * @funtion      : Get_Dog_Clock  
  * @LastEditors  : JackyJuu
  * @description  : 获取机器狗模式时钟，反馈设置间隔市场情况
  * @param         {int} Set_Time 设置市场，1000为1000ms一次触发
  * @param         {int} Reset_Flag 重置标志，flag为1计算单位重置一次
  * @return        {*}
  *******************************************************************************/
int Get_Dog_Clock(int Set_Time,int Reset_Flag) 
{
    static int Count_Data = 0;
    if(Reset_Flag == 1)
    {
        Count_Data = 0;
    }
    if((Count_Data * Dog_Delay_Time_Set) >= (Set_Time))
    {
        Count_Data = 0;
        return 1;
    }
    Count_Data++;
    return 0;
}

/*******************************************************************************
  * @funtion      : Leg_Order_Motion_Celebrate_Ready  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗单腿校准运动ready状态相关haha念书
  * @param         {Dog_Leg_Data_Struct*} Dog_Leg_Ready 机器狗腿部数据结构体地址
  * @return        {*}
  *******************************************************************************/
Leg_Celebrate_State_Enum Leg_Order_Motion_Celebrate_Ready(Dog_Leg_Data_Struct* Dog_Leg_Ready)
{
    Single_Leg_Set_PowerLess(Dog_Leg_Ready,0);
    Single_Leg_Get_Servo_Data(Dog_Leg_Ready);
    if((Dog_Leg_Ready->Leg_Motor_Message[0]->load == 0) &&\
        (Dog_Leg_Ready->Leg_Motor_Message[1]->load == 0) &&\
        (Dog_Leg_Ready->Leg_Motor_Message[2]->load == 0))
        {
            return Leg_OK;
        }
    return Leg_Ready;
}

/*******************************************************************************
  * @funtion      : Leg_Order_Motion_Celebrate_Motion_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗单腿校准动作状态设置相关函数
  * @param         {Dog_Leg_Data_Struct*} Dog_Leg_Ready 机器狗腿部数据结构体地址
  * @param         {Leg_Celebrate_State_Enum*} Leg_Celebrate_State 获取当前指令模式校准状态数据地址
  * @return        {*}
  *******************************************************************************/
void Leg_Order_Motion_Celebrate_Motion_Set(Dog_Leg_Data_Struct* Dog_Leg_Ready,Leg_Celebrate_State_Enum* Leg_Celebrate_State)
{
    static Leg_Loc_Enum Get_Leg_Last_Loc;
    static Leg_Celebrate_State_Enum Get_Last_Celebrate_State;
    static int Get_Leg_Data[3];
    if(*Leg_Celebrate_State == Leg_Butt_Celebrate)
    {
        if(Get_Dog_Clock(1000,0) == 1)
        {
            Usb_Write_Data("{\"type\":\"dog-celebrate\",\"state\":\"leg_butt_celebrate\"}\r\n");
        }
        Servo_Read_Position(Dog_Leg_Ready->Leg_Motor_Message[0]->id);
        Get_Leg_Data[0] = Dog_Leg_Ready->Leg_Motor_Message[0]->position;
    }
    else if(*Leg_Celebrate_State == Leg_Thigh_Celebrate)
    {
        if(Get_Dog_Clock(1000,0) == 1)
        {
            Usb_Write_Data("{\"type\":\"dog-celebrate\",\"state\":\"leg_thigh_celebrate\"}\r\n");
        }
        if(Get_Last_Celebrate_State == Leg_Butt_Celebrate)
        {
            Servo_Sync_Write_Load(&Dog_Leg_Ready->Leg_Motor_Message[0]->id, 1, 1);
            Dog_Leg_Ready->Leg_Offset_Data[0] = Get_Leg_Data[0];
        }
        Servo_Read_Position(Dog_Leg_Ready->Leg_Motor_Message[1]->id);
        Get_Leg_Data[1] = Dog_Leg_Ready->Leg_Motor_Message[1]->position;      
    }
    else if(*Leg_Celebrate_State == Leg_Calf_Celebrate)
    {
        if(Get_Dog_Clock(1000,0) == 1)
        {
            Usb_Write_Data("{\"type\":\"dog-celebrate\",\"state\":\"leg_calf_celebrate\"}\r\n");
        }
        if(Get_Last_Celebrate_State == Leg_Thigh_Celebrate)
        {
            Servo_Sync_Write_Load(&Dog_Leg_Ready->Leg_Motor_Message[1]->id, 1, 1);
            Dog_Leg_Ready->Leg_Offset_Data[1] = Get_Leg_Data[1];
        }
        Servo_Read_Position(Dog_Leg_Ready->Leg_Motor_Message[2]->id);
        Get_Leg_Data[2] = Dog_Leg_Ready->Leg_Motor_Message[1]->position - Dog_Leg_Ready->Leg_Motor_Message[2]->position;              
    }
    else if(*Leg_Celebrate_State == Leg_Celebrate_Done)
    {
        if(Get_Last_Celebrate_State == Leg_Calf_Celebrate)
        {
            Dog_Leg_Ready->Leg_Offset_Data[2] = Get_Leg_Data[2];
        }
        if( Dog_Leg_Ready->Leg_Offset_Data[2] == Get_Leg_Data[2])
        {
            *Leg_Celebrate_State = Leg_OK;   
        }           
    }
    Get_Leg_Last_Loc = Dog_Leg_Ready->Leg_Loc;
    Get_Last_Celebrate_State = *Leg_Celebrate_State;
}



/*******************************************************************************
  * @funtion      : Dog_Order_Motion_Celebrate_Data_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗指令模式校准动作相关数值设置
  * @param         {Lite_Dog_Struct*} Dog_MCD 机器狗相关设置数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Dog_Order_Motion_Celebrate_Data_Set(Lite_Dog_Struct* Dog_MCD)
{
    static Leg_Celebrate_State_Enum Last_Leg_Celebrate_state;
    static int Get_Pos[3][2];

    if(Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State != Last_Leg_Celebrate_state)
    {
        Get_Dog_Clock(1000,1);
    }
    if(Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State == Leg_Ready)
	{
        if(Get_Dog_Clock(1000,0) == 1)
        {
            Usb_Write_Data("{\"type\":\"dog-celebrate\",\"state\":\"leg_ready\"}\r\n");
        }
		switch(Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_Loc_Set)
		{
			case Dog_RB_Leg:
                if(Leg_Order_Motion_Celebrate_Ready(&Dog_MCD->Dog_RB_Leg_Data) == Leg_OK)
                {
                    Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State = Leg_Butt_Celebrate;
                }
			break;
            default:
            break;
		}
	}
    if((Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State == Leg_Butt_Celebrate) ||\
    (Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State == Leg_Thigh_Celebrate) ||\
    (Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State == Leg_Calf_Celebrate) ||\
    (Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State == Leg_Celebrate_Done))
    {
        switch(Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_Loc_Set)
		{
			case Dog_LF_Leg:
                Leg_Order_Motion_Celebrate_Motion_Set(&Dog_MCD->Dog_LF_Leg_Data,&Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State);
			break;
            case Dog_RF_Leg:
                Leg_Order_Motion_Celebrate_Motion_Set(&Dog_MCD->Dog_RF_Leg_Data,&Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State);
			break;
            case Dog_LB_Leg:
                Leg_Order_Motion_Celebrate_Motion_Set(&Dog_MCD->Dog_RB_Leg_Data,&Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State);
			break;
            case Dog_RB_Leg:
                Leg_Order_Motion_Celebrate_Motion_Set(&Dog_MCD->Dog_RB_Leg_Data,&Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State);
			break;
            default:
            break;
		}
    }

	Last_Leg_Celebrate_state = Dog_MCD->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State;
}


/*******************************************************************************
  * @funtion      : Dog_Order_Motion_Data_Update  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗指令模式更新函数
  * @param         {Lite_Dog_Struct*} Dog_Order_MMU 获取机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Dog_Order_Motion_Data_Update(Lite_Dog_Struct* Dog_Order_MMU)
{
	static Dog_Order_Set_Mode_Struct Dog_Order_Set_Mode_Last;
    static Dog_Order_Set_Mode_Struct Dog_Order_Celebrate_Last;
    static Dog_Move_Mode_Enum Dog_Move_Mode_Last;

    switch(Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode)
    {
        //指令运动模式
        case Dog_Order_Move_Mode:
            Dog_Data->Dog_State = Dog_OK;
            if(Dog_Order_Set_Mode_Last != Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode)
            {
                Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Move_Mode = Stop_M;
            }
            else
            {	
				Dog_Order_Motion_Move_Data_Set(Dog_Order_MMU);	
        	}
        break;
        //指令自稳模式
        case Dog_Order_Stable_Mode:
            Dog_Data->Dog_State = Dog_OK;
			if(Dog_Order_Set_Mode_Last != Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode)
            {
                Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Move_Mode = Stop_M;
            }
            else
            {	
				Dog_Order_Adaptive_Angle_Mode_Set(Dog_Order_MMU);
			}
        break;
        //指令校准模式
        case Dog_Order_Celebrate_Mode:
            Dog_Data->Dog_State = Dog_Celebrate;
			if(Dog_Order_Set_Mode_Last != Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode)
            {
                Dog_Order_Celebrate_Last = Dog_Order_Set_Mode_Last;
                Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Move_Mode = Stop_M;
            }
            else
            {	
                if(Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State == Leg_OK)
                {
                    Lite_Dog_Write_Flash_Data(Dog_Order_MMU);
                    Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode = Dog_Order_Celebrate_Last;
                    Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Leg_Celebrate_State = Leg_Ready;
                }
                else
				    Dog_Order_Motion_Celebrate_Data_Set(Dog_Order_MMU);
			}
        break;
        //指令自动模式
        case Dog_Order_Auto_Mode:
			// if(Dog_Order_Set_Mode_Last != Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode)
            // {
            //     Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Move_Mode = Stop_M;
            // }
            // else
            {	
				// switch(Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Auto)
				// {
				// case PushUp_Auto:
				// 	Lite_Dog_Action_PushUp(4,Dog_Order_MMU);
				// break;
				// case SayHello_Auto:
				// 	Lite_Dog_Action_SayHello(6,1,Dog_Order_MMU);
				// break;
				// case Dance_Auto:
				// 	Lite_Dog_Action_Dance(4,1,Dog_Order_MMU);
				// break;
				// case Pee_Auto:
				// 	Lite_Dog_Action_Pee(6,2,Dog_Order_MMU);
				// break;
				// }
				// Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Move_Mode = Stop_M;
				// Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode = Dog_Order_Move_Mode;
			}
        break;
    }
    Dog_Order_Set_Mode_Last = Dog_Order_MMU->Dog_Control_Data.Dog_Order_Mode_Data->Dog_Order_Set_Mode;

}

